from pyfbsdk import *

app=FBApplication()
system=FBSystem()
scene=system.Scene

SkeletonNodeIds=[
  FBBodyNodeId.kFBReferenceNodeId,
  FBBodyNodeId.kFBHipsNodeId,
  FBBodyNodeId.kFBWaistNodeId,
  FBBodyNodeId.kFBChestNodeId,
  FBBodyNodeId.kFBSpine2NodeId,
  FBBodyNodeId.kFBSpine3NodeId,
  FBBodyNodeId.kFBNeckNodeId,
  FBBodyNodeId.kFBHeadNodeId,
  FBBodyNodeId.kFBRightCollarNodeId,
  FBBodyNodeId.kFBRightShoulderNodeId,
  FBBodyNodeId.kFBRightElbowNodeId,
  FBBodyNodeId.kFBRightElbowRollNodeId,
  FBBodyNodeId.kFBRightWristNodeId,
  FBBodyNodeId.kFBRightThumbANodeId,
  FBBodyNodeId.kFBRightThumbBNodeId,
  FBBodyNodeId.kFBRightThumbCNodeId,
  FBBodyNodeId.kFBRightThumbDNodeId,
  FBBodyNodeId.kFBRightIndexANodeId,
  FBBodyNodeId.kFBRightIndexBNodeId,
  FBBodyNodeId.kFBRightIndexCNodeId,
  FBBodyNodeId.kFBRightIndexDNodeId,
  FBBodyNodeId.kFBRightMiddleANodeId,
  FBBodyNodeId.kFBRightMiddleBNodeId,
  FBBodyNodeId.kFBRightMiddleCNodeId,
  FBBodyNodeId.kFBRightMiddleDNodeId,
  FBBodyNodeId.kFBRightRingANodeId,
  FBBodyNodeId.kFBRightRingBNodeId,
  FBBodyNodeId.kFBRightRingCNodeId,
  FBBodyNodeId.kFBRightRingDNodeId,
  FBBodyNodeId.kFBRightPinkyANodeId,
  FBBodyNodeId.kFBRightPinkyBNodeId,
  FBBodyNodeId.kFBRightPinkyCNodeId,
  FBBodyNodeId.kFBRightPinkyDNodeId,
  FBBodyNodeId.kFBLeftCollarNodeId,
  FBBodyNodeId.kFBLeftShoulderNodeId,
  FBBodyNodeId.kFBLeftElbowNodeId,
  FBBodyNodeId.kFBLeftElbowRollNodeId,
  FBBodyNodeId.kFBLeftWristNodeId,
  FBBodyNodeId.kFBLeftThumbANodeId,
  FBBodyNodeId.kFBLeftThumbBNodeId,
  FBBodyNodeId.kFBLeftThumbCNodeId,
  FBBodyNodeId.kFBLeftThumbDNodeId,
  FBBodyNodeId.kFBLeftIndexANodeId,
  FBBodyNodeId.kFBLeftIndexBNodeId,
  FBBodyNodeId.kFBLeftIndexCNodeId,
  FBBodyNodeId.kFBLeftIndexDNodeId,
  FBBodyNodeId.kFBLeftMiddleANodeId,
  FBBodyNodeId.kFBLeftMiddleBNodeId,
  FBBodyNodeId.kFBLeftMiddleCNodeId,
  FBBodyNodeId.kFBLeftMiddleDNodeId,
  FBBodyNodeId.kFBLeftRingANodeId,
  FBBodyNodeId.kFBLeftRingBNodeId,
  FBBodyNodeId.kFBLeftRingCNodeId,
  FBBodyNodeId.kFBLeftRingDNodeId,
  FBBodyNodeId.kFBLeftPinkyANodeId,
  FBBodyNodeId.kFBLeftPinkyBNodeId,
  FBBodyNodeId.kFBLeftPinkyCNodeId,
  FBBodyNodeId.kFBLeftPinkyDNodeId,
  FBBodyNodeId.kFBRightHipNodeId,
  FBBodyNodeId.kFBRightKneeNodeId,
  FBBodyNodeId.kFBRightAnkleNodeId,
  FBBodyNodeId.kFBRightFootNodeId,
  FBBodyNodeId.kFBRightExtraFootFingerANodeId,
  FBBodyNodeId.kFBLeftHipNodeId,
  FBBodyNodeId.kFBLeftKneeNodeId,
  FBBodyNodeId.kFBLeftAnkleNodeId,
  FBBodyNodeId.kFBLeftFootNodeId,
  FBBodyNodeId.kFBLeftExtraFootFingerANodeId
]

MOBU_BODYNODE_NAME=[
"Hips",
"Spine",
"LeftShoulder",
"LeftArm",
"LeftForeArm",
"LeftHand",
"Neck",
"Head",
"RightShoulder",
"RightArm",
"RightForeArm",
"RightHand",
"LeftUpLeg",
"LeftLeg",
"LeftFoot",
"RightUpLeg",
"RightLeg",
"RightFoot"
]

MOBU_BODYNODE_ID=[
FBBodyNodeId.kFBHipsNodeId,
FBBodyNodeId.kFBWaistNodeId,
FBBodyNodeId.kFBLeftCollarNodeId,
FBBodyNodeId.kFBLeftShoulderNodeId,
FBBodyNodeId.kFBLeftElbowNodeId,
FBBodyNodeId.kFBLeftWristNodeId,
FBBodyNodeId.kFBNeckNodeId,
FBBodyNodeId.kFBHeadNodeId,
FBBodyNodeId.kFBRightCollarNodeId,
FBBodyNodeId.kFBRightShoulderNodeId,
FBBodyNodeId.kFBRightElbowNodeId,
FBBodyNodeId.kFBRightWristNodeId,
FBBodyNodeId.kFBLeftHipNodeId,
FBBodyNodeId.kFBLeftKneeNodeId,
FBBodyNodeId.kFBLeftAnkleNodeId,
FBBodyNodeId.kFBRightHipNodeId,
FBBodyNodeId.kFBRightKneeNodeId,
FBBodyNodeId.kFBRightAnkleNodeId
]

def getCharacter(name):
    return [c for c in scene.Characters if c.Name==name][0]
    
def printTPose(target):
    empty_nodes=[]
    
    T=FBVector3d()
    R=FBVector3d()
    
    for index, name in enumerate(MOBU_BODYNODE_NAME):
        bodynode_id = MOBU_BODYNODE_ID[index]
        model = target.GetModel(bodynode_id)
        if model:
            model.GetVector(T, FBModelTransformationType.kModelTranslation, True)
            model.GetVector(R, FBModelTransformationType.kModelRotation, True)
            
            print 'mChannelData[i][DATA_TX]=',T[0],';'
            print 'mChannelData[i][DATA_TY]=',T[1],';'
            print 'mChannelData[i][DATA_TZ]=',T[2],';'
            print 'mChannelData[i][DATA_RX]=',R[0],';'
            print 'mChannelData[i][DATA_RY]=',R[1],';'
            print 'mChannelData[i][DATA_RZ]=',R[2],';'
            
            print '\n++i;'
        else:
            empty_nodes.append(name)
    if len(empty_nodes):
        print '\n\n',empty_nodes
    
printTPose(getCharacter('wh'))

